using System;
using System.Collections.Generic;
using System.Text;
using Cambia.CoreLib;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  public class DepthSensor : Sensor
  {
    public override Measurement PredictForSimulation(WorldState worldState, Map map)
    {
      if (Globals.selfLocatorProperties.SensorNoise == NoiseType.noNoise)
        return PredictPerfect(worldState, map);
      else
      {
        double sigma = Globals.depthMeasurementSigma;
        double gaussianError = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);

        DepthMeasurement measurement = (DepthMeasurement)PredictPerfect(worldState, map);
        double newDepth;
        if (Globals.selfLocatorProperties.SensorNoise == NoiseType.noGaussianButSystematic)
        {
          newDepth = measurement.Depth + 20.0;
        }
        else
        {
          newDepth = measurement.Depth + gaussianError;
        }
        if (newDepth < 0) newDepth = 0;
        if (newDepth > 255) newDepth = 255;
        return new DepthMeasurement(newDepth, this);
      }
    }

    public override Measurement PredictPerfect(WorldState worldState, Map map)
    {
      double x_robot = 0;
      double y_robot = 0;

      if (worldState is WorldState1D)
      {
        x_robot = (worldState as WorldState1D).Position;
      }

      if (worldState is WorldState2D)
      {
        x_robot = (worldState as WorldState2D).Vector.x;
        y_robot = (worldState as WorldState2D).Vector.y;
      }

      if (worldState is WorldStatePose2D)
      {
        x_robot = (worldState as WorldStatePose2D).Pose.translation.x;
        y_robot = (worldState as WorldStatePose2D).Pose.translation.y;
      }

      double depth = 0;

      if (map is MapWithDepth)
      {
        depth = (map as MapWithDepth).GetDepth(x_robot, y_robot);
      }
      return new DepthMeasurement(depth, this);
    }

    public override double GetMeasurementProbability(WorldState worldState, Map map, Measurement measurement)
    {
      double expectedMeasurement = (PredictPerfect(worldState, map) as DepthMeasurement).Depth;
      double conductedMeasurement = (measurement as DepthMeasurement).Depth;
      return Globals.Gaussian(conductedMeasurement, Globals.distanceMeasurementSigma, expectedMeasurement);
    }
/*
    public override void Draw(System.Drawing.Graphics graphics, Measurement measurement, WorldState worldState, double shade)
    {
      double startX = 0;
      double startY = 0;
      double depth = (measurement as DepthMeasurement).Depth;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }

      Color color = Color.FromArgb((int)depth, (int)depth, (int)depth);
      Pen pen = new Pen(color, Globals.rasterWidth);
      double radius = Globals.rasterWidth / 2;
      if (Tools.DebugKeyTool.IsActive("options:show depth measurement as line"))
        graphics.DrawLine(pen, (float)(startX - radius), (float)(startY), (float)(startX + radius), (float)(startY));
      SolidBrush brush = new SolidBrush(Color.Orange);
      Font font = new Font("Arial", 13, FontStyle.Bold);
      //graphics.DrawString(this.depth.ToString(), font, brush, (float)startX, (float)startY);
      Pen grayPen = new Pen(Color.Gray, 2);
      graphics.DrawLine(grayPen, (float)startX, (float)startY, (float)startX - Globals.rasterWidth, (float)startY + Globals.rasterWidth);
      if (DebugKeyTool.IsActive("export:save everything to emf"))
        graphics.DrawString(depth.ToString("0"), font, brush, (float)(startX - Globals.rasterWidth * 2), (float)(startY + Globals.rasterWidth * 0.5));
      else
        DrawingTools.DrawFlippedText(graphics, depth.ToString("0"), font, brush, (float)startX - Globals.rasterWidth, (float)startY + Globals.rasterWidth);
    }
*/
    public override void Draw(Measurement measurement, WorldState worldState, double shade, string drawingID)
    {
      double startX = 0;
      double startY = 0;
      double depth = (measurement as DepthMeasurement).Depth;
      if (worldState is WorldState1D)
      {
        startX = (worldState as WorldState1D).Position;
        startY = 0;
      }
      if (worldState is WorldState2D)
      {
        startX = (worldState as WorldState2D).Vector.x;
        startY = (worldState as WorldState2D).Vector.y;
      }
      if (worldState is WorldStatePose2D)
      {
        startX = (worldState as WorldStatePose2D).Pose.translation.x;
        startY = (worldState as WorldStatePose2D).Pose.translation.y;
      }

      Color color = Color.FromArgb((int)depth, (int)depth, (int)depth);
      double radius = Globals.rasterWidth / 2;
      if (Tools.DebugKeyTool.IsActive("options:show depth measurement as line"))
      {
        DebugDrawingTool.CreateLine(drawingID,
          new Vector2(startX - radius, startY),
          new Vector2(startX + radius, startY), Globals.rasterWidth, color);
      }
      SolidBrush brush = new SolidBrush(Color.Orange);
      DebugDrawingTool.CreateLine(drawingID, 
        new Vector2(startX, startY), 
        new Vector2(startX - Globals.rasterWidth, startY + Globals.rasterWidth), 2, Color.Gray);

      DebugDrawingTool.CreateText(drawingID, depth.ToString("0"),
        new Vector2(startX - 2*Globals.rasterWidth, startY + Globals.rasterWidth), 15, Color.Red);
    }

    public override string ToString()
    {
      return this.GetType().Name;
    }
  }
}
